/*
 * Copyright (c) 1997, 1998, 2003, 2006 Aleksandar Samardzic
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <assert.h>		/* for assert() function */
#include <stdlib.h>		/* for calloc() function */
#include "triangle.h"
#include "edge.h"
#include "data_access_structure.h"
#include "voxel_grid.h"

/* Triangle_Intersect - intersect triangle with a line */
Bool
Triangle_Intersect(this, pLine)
     PTriangle      *this;
     PLine          *pLine;
{
	double         *p0,
	               *dp = pLine->dp;
	double          A = this->plane.A,
	    B = this->plane.B,
	    C = this->plane.C,
	    d = this->plane.d;
	double          t;
	double          den;
	double         *point0,
	                point[3];
	double         *P,
	               *Q;
	double          p,
	                q;
	int             axis;

	/* intersect line with triangle plane */
	den = A * dp[X] + B * dp[Y] + C * dp[Z];

	if (den == 0)		/* line parallel to triangle plane */
		return FALSE;

	/* check if t is in range [EPSILON,pLine->t-EPSILON) */
	p0 = pLine->p0;
	t = (d - (A * p0[X] + B * p0[Y] + C * p0[Z])) / den;
	if (t < EPSILON || t >= pLine->t - EPSILON)
		return FALSE;

	/* calculate point where line crosses triangle plane */
	point0 = this->vertex[0].point;
	for (axis = X; axis <= Z; axis++)
		point[axis] = p0[axis] + t * dp[axis] - point0[axis];

	/* calculate barycentric coordinates of crossing point */
	P = this->P, Q = this->Q;
	p = P[X] * point[X] + P[Y] * point[Y] + P[Z] * point[Z];
	q = Q[X] * point[X] + Q[Y] * point[Y] + Q[Z] * point[Z];

	/* check if crossing point is in triangle */
	if (p < 0 || q < 0 || p + q > 1)
		return FALSE;

	pLine->t = t;
	this->p = p, this->q = q;	/* remember barycentric
					 * coordinates of crossing point */
	return TRUE;
}

/* Triangle_CalcNormal - calculate triangle normal at specified point */
void
Triangle_CalcNormal(this, point, normal)
     PTriangle      *this;
     PVector         point;
     PVector         normal;
{
	double          p = this->p,
	    q = this->q;
	double         *normal0 = this->vertex[0].normal;
	double         *normal1 = this->vertex[1].normal;
	double         *normal2 = this->vertex[2].normal;
	double          mul,
	                den;

	/* use saved barycentric coordinates of intersection point to
	 * interpolate normal */
	mul = 1 - (p + q);
	normal[X] = mul * normal0[X] + p * normal1[X] + q * normal2[X];
	normal[Y] = mul * normal0[Y] + p * normal1[Y] + q * normal2[Y];
	normal[Z] = mul * normal0[Z] + p * normal1[Z] + q * normal2[Z];
	Vector_Normalize(normal, mul, den);
}

/* Triangle_CalcBoundingVolume - calculate bounding volume for triangle */
void
Triangle_CalcBoundingVolume(this, pNode, pSlabs)
     PTriangle      *this;
     PBoundingVolume *pNode;
     PSlabs         *pSlabs;
{
	PVector        *pSlabVectors = pSlabs->pVectors;
	Byte            slab,
	                numSlabs = pSlabs->numSlabs;
	DWord           vertex;
	PVertex        *pVertex = this->vertex;
	double          dCurr;

	/* initialize bounding volume */
	for (slab = 0; slab < numSlabs; slab++) {
		pNode->d[slab][LO] = DBL_MAX;
		pNode->d[slab][HI] = -DBL_MAX;
	}

	for (vertex = 0; vertex < 3; vertex++, pVertex++)	/* for
								 * each
								 * vertex */
		for (slab = 0; slab < numSlabs; slab++) {	/* for
								 * each
								 * slab */
			/* calculate current value of d */
			dCurr =
			    Vector_DotProduct(pVertex->point,
					      pSlabVectors[slab]);

			/* adjust bounding volume if necessary */
			if (dCurr < pNode->d[slab][LO])
				pNode->d[slab][LO] = dCurr;
			if (dCurr > pNode->d[slab][HI])
				pNode->d[slab][HI] = dCurr;
		}
}

/* Triangle_Voxelize - voxelization algorithm for triangle according to
 * [Kauf87] */
void
Triangle_Voxelize(this, pGrid)
     PTriangle      *this;
     PVoxelGrid     *pGrid;
{
	DWord           numVertices = 3;
	double          (*d)[2] = pGrid->d;
	double          hCell = pGrid->hCell,
	    _hCell = 1 / hCell;
	double          A,
	                B,
	                C;
	Byte            u,
	                v,
	                w;
	PVertex        *pVertex0,
	               *pVertex1,
	               *pEndVertex;
	double         *v0,
	               *v1;
	PVector         p0,
	                p;
	DWord           voxel[2],
	                voxel0,
	                voxel1,
	                voxel_curr;
	double          t,
	                mul;
	DWord          *numETEdges,
	                numAETEdges,
	                gap;
	PEdge          *pEdge,
	              **ppEdge;
	PEdge        ***ET,
	              **ppETEdge,
	              **ppEndETEdge;
	PEdge         **AET,
	              **AETPrev,
	              **ppAETEdge,
	              **ppEndAETEdge,
	               *pAETEdge;
	double          v_curr;
	Bool            sorted;

	A = fabs(this->plane.A), B = fabs(this->plane.B), C =
	    fabs(this->plane.C);

	if (A <= B)
		if (A <= C) {
			u = X;
			if (B <= C)
				v = Y, w = Z;
			else
				v = Z, w = Y;
		} else
			u = Z, v = X, w = Y;
	else if (B <= C) {
		u = Y;
		if (A <= C)
			v = X, w = Z;
		else
			v = Z, w = X;
	} else
		u = Z, v = Y, w = X;

	ET = calloc(pGrid->numCells[v], sizeof(PEdge **));
        assert(ET != NULL);
	numETEdges = calloc(pGrid->numCells[v], sizeof(DWord));
        assert(numETEdges != NULL);
	voxel[LO] = pGrid->numCells[v], voxel[HI] = 0;
	for (pVertex1 = &(this->vertex[0]), pEndVertex =
	     pVertex1 + numVertices, pVertex0 = pEndVertex - 1;
	     pVertex1 < pEndVertex; pVertex0 = pVertex1, pVertex1++) {
		pEdge = malloc(sizeof(PEdge));
		assert(pEdge != NULL);
		if (pVertex0->point[v] < pVertex1->point[v])
			v0 = pVertex0->point, v1 = pVertex1->point;
		else
			v0 = pVertex1->point, v1 = pVertex0->point;

		Vector_Assign(p0, v0);
		Vector_Sub(p, v1, v0);
		t = 1;
		VoxelGrid_EnumerateCellsLine(pGrid, p0, p, &t,
					     VoxelGrid_Insert, this);

		voxel0 = (DWord) ((v0[v] - d[v][LO]) * _hCell);
		voxel1 = (DWord) ((v1[v] - d[v][LO]) * _hCell);
		if (voxel0 != voxel1) {
			Vector_Assign(p0, v0);
			Vector_Sub(p, v1, v0);
			mul = 1 / p[v];
			t = (d[v][LO] + (voxel0 + 1) * hCell -
			     v0[v]) * mul;
			Vector_LERP(pEdge->point, p0, p, t);
			mul *= hCell;
			Vector_Mul(pEdge->delta, p, mul);
			pEdge->hi = voxel1;

			if (voxel0 < voxel[LO])
				voxel[LO] = voxel0;
			if (voxel1 > voxel[HI])
				voxel[HI] = voxel1;

			ET[voxel0] =
			    (PEdge **) realloc(ET[voxel0],
					       (++(numETEdges[voxel0])) *
					       sizeof(PEdge **));
			assert(ET[voxel0] != NULL);
			ppETEdge = ET[voxel0], ppEndETEdge =
			    ppETEdge + numETEdges[voxel0] - 1;
			for (; ppETEdge < ppEndETEdge; ppETEdge++)
				if ((*ppETEdge)->point[v] >
				    pEdge->point[v])
					break;
			memcpy(ppETEdge + 1, ppETEdge,
			       (ppEndETEdge - ppETEdge) * sizeof(PEdge *));
			*ppETEdge = pEdge;
		} else
			free(pEdge);
	}

	AET = NULL, numAETEdges = 0;
	for (voxel_curr = voxel[LO] + 1; voxel_curr <= voxel[HI];
	     voxel_curr++) {
		v_curr = d[v][LO] + voxel_curr * hCell;

		ppETEdge = ET[voxel_curr - 1], ppEndETEdge =
		    ppETEdge + numETEdges[voxel_curr - 1];
		AETPrev = AET, ppAETEdge = AETPrev, ppEndAETEdge =
		    ppAETEdge + numAETEdges;
		numAETEdges += numETEdges[voxel_curr - 1];
		AET = malloc(numAETEdges * sizeof(PEdge *));
		assert(AET != NULL);
		for (ppEdge = AET;
		     ppAETEdge < ppEndAETEdge && ppETEdge < ppEndETEdge;
		     ppEdge++)
			if ((*ppAETEdge)->point[v] < (*ppETEdge)->point[v])
				*ppEdge = *(ppAETEdge++);
			else
				*ppEdge = *(ppETEdge++);
		for (; ppAETEdge < ppEndAETEdge; ppEdge++)
			*ppEdge = *(ppAETEdge++);
		for (; ppETEdge < ppEndETEdge; ppEdge++)
			*ppEdge = *(ppETEdge++);
		free(ET[voxel_curr - 1]);
		free(AETPrev);

		for (ppAETEdge = AET, ppEndAETEdge = AET + numAETEdges;
		     ppAETEdge < ppEndAETEdge; ppAETEdge += 2) {
			if ((*ppAETEdge)->point[u] <
			    (*(ppAETEdge + 1))->point[u])
				v0 = (*ppAETEdge)->point, v1 =
				    (*(ppAETEdge + 1))->point;
			else
				v0 = (*(ppAETEdge + 1))->point, v1 =
				    (*ppAETEdge)->point;

			Vector_Assign(p0, v0);
			Vector_Sub(p, v1, v0);
			t = 1;
			p0[v] -= hCell / 2;
			VoxelGrid_EnumerateCellsLine(pGrid, p0, p, &t,
						     VoxelGrid_Insert,
						     this);
			p0[v] += hCell;
			VoxelGrid_EnumerateCellsLine(pGrid, p0, p, &t,
						     VoxelGrid_Insert,
						     this);
			p0[v] -= hCell / 2;
		}

		for (gap = 0, ppAETEdge = AET, ppEndAETEdge =
		     AET + numAETEdges; ppAETEdge < ppEndAETEdge;
		     ppAETEdge++)
			if ((pAETEdge = *ppAETEdge)->hi == voxel_curr) {
				numAETEdges--;
				free(pAETEdge);
				gap++;
			} else {
				Vector_AddAssign(pAETEdge->point,
						 pAETEdge->delta)
				    * (ppAETEdge - gap) = *ppAETEdge;
			}
		if (AET = realloc(AET, numAETEdges * sizeof(PEdge *)))
			FOREVER {
			sorted = TRUE;
			for (ppAETEdge = AET, ppEndAETEdge =
			     AET + numAETEdges - 1;
			     ppAETEdge < ppEndAETEdge; ppAETEdge++)
				if ((*ppAETEdge)->point[v] >
				    (*(ppAETEdge + 1))->point[v]) {
					sorted = FALSE;
					pEdge = *ppAETEdge;
					*ppAETEdge = *(ppAETEdge + 1);
					*(ppAETEdge + 1) = pEdge;
				}
			if (sorted)
				break;
			}
	}

	free(AET);
	free(numETEdges);
	free(ET);
}

/* Triangle_CalcCoeffs - precompute coefficients for later speed
 * barycentric coordinates calculation */
void
Triangle_CalcCoeffs(this)
     PTriangle      *this;
{
	PVector         P,
	                Q;
	double          p[3],
	                q[3];
	double          _P,
	                _Q,
	                mul_p,
	                mul_q;
	double          mul,
	                mul2;

	Vector_Sub(P, this->vertex[1].point, this->vertex[0].point);
	Vector_Sub(Q, this->vertex[2].point, this->vertex[0].point);

	_P = Vector_Norm(P), mul_p = 1 / _P;
	Vector_Mul(p, P, mul_p);
	_Q = Vector_Norm(Q), mul_q = 1 / _Q;
	Vector_Mul(q, Q, mul_q);

	mul = Vector_DotProduct(p, q);

	Vector_Mul(P, q, -mul);
	Vector_AddAssign(P, p);
	Vector_Mul(Q, p, -mul);
	Vector_AddAssign(Q, q);

	mul2 = 1 - mul * mul, mul_p = 1 / (_P * mul2), mul_q =
	    1 / (_Q * mul2);
	Vector_MulAssign(P, mul_p);
	Vector_MulAssign(Q, mul_q);
	Vector_Assign(this->P, P);
	Vector_Assign(this->Q, Q);
}
